from sensor_msgs.msg import LaserScan
from floor_set.msg import Floor
from std_msgs.msg import String
from lib.calcPoints import calcPoints
from lib.getCenters import getCenters
from lib.PID import PIDController
from lib.tool import calcRealAngle, calcValueAngle, calcSpeedByAngle
import rospy
import math
import os
import sys
import termios
import tty
import cv2
import json
import time
import atexit

# 常量
TTY_PATH = "/dev/tty"
STTY_US = "stty raw -echo -F"
STTY_DEF = "stty -raw echo -F"
STOP_DELAY = 5
STOP_UNTIL = -1
MID_ANGLE = 80
validDistance = 400
Pi = 3.1415926
DBSCAN_CONFIG = {
    "eps": 25,
    "min_samples": 5,
    "debug": False,
    "size": (validDistance, validDistance),
}

# PID arguments
kp = 0.755
ki = 0.012
kd = 0.05

pid = PIDController(-kp, -ki, -kd)

FLAG_STOP = False
isDataReceived: bool = False
steeringAngle: int = 0
# inputChar = 0
currentSpeed: int = 2080
speedRange = 50
isTest: bool = False
headlight: bool = False


def CamCtlCallback(msg) -> None:
    global STOP_UNTIL, headlight
    headlight = False
    STOP_UNTIL = time.time() + STOP_DELAY
    msg = json.loads(msg.data)


def TestCtlCallback(msg) -> None:
    global STOP_UNTIL, headlight, kp, ki, kd, pid, FLAG_STOP
    headlight = not headlight
    msg = json.loads(msg.data)
    kp = msg.get("kp", kp)
    ki = msg.get("ki", ki)
    kd = msg.get("kd", kd)
    FLAG_STOP = msg.get("stop", False)
    
    pid = PIDController(-kp, -ki, -kd)


def LidarCallback(msg: LaserScan) -> None:
    """
    Lidar Calback function

    Args:
        msg: LaserScan for senser.msg
    Returns:
        None
    """
    # start_time = time.time()

    global isDataReceived, steeringAngle, STOP_UNTIL

    if time.time() < STOP_UNTIL:
        return

    isDataReceived = True

    foundLeftObstacle = 0
    foundRightObstacle = 0

    data = list(map(lambda x: x * 100, msg.ranges[:1000]))
    points = calcPoints(data)

    centers, img = getCenters(points, validDistance, **DBSCAN_CONFIG)

    if DBSCAN_CONFIG["debug"]:
        cv2.imshow("DEBUG", img)
        cv2.waitKey(1)

    closestCenters = [validDistance, validDistance]
    closestDistances = [validDistance, validDistance]
    closestAngles = [0, 0]

    for center in centers:
        x = center[0]
        y = center[1]
        distance = math.sqrt(x * x + y * y)
        angle = math.degrees(math.atan2(y, x))
        if distance > validDistance or y < 0:
            continue

        if x < 0:
            if distance < closestDistances[0]:
                closestCenters[0] = center
                closestDistances[0] = distance
                closestAngles[0] = angle - 90
                if closestAngles[0] < 30:
                    continue
                foundLeftObstacle = 1
        elif x > 0:
            if distance < closestDistances[1]:
                closestCenters[1] = center
                closestDistances[1] = distance
                closestAngles[1] = 90 - angle
                if closestAngles[1] < 30:
                    continue
                foundRightObstacle = 1

    if foundLeftObstacle and foundRightObstacle:
        value = abs(closestCenters[1][0]) - abs(closestCenters[0][0])
        steeringAngle = pid.calculate(0, value)
    elif foundLeftObstacle and not foundRightObstacle:
        value = abs(closestCenters[0][0])
        steeringAngle = pid.calculate(0, value)
        print("[Info] Left MAX")
    elif not foundLeftObstacle and foundRightObstacle:
        value = -abs(closestCenters[1][0])
        steeringAngle = pid.calculate(0, value)
        print("[Info] Right MAX")
    else:
        steeringAngle = 0
        print("[Inof] No obstacle")

    # print(f"{round(time.time() - start_time,2)} Hz")


rospy.init_node("lidar_node")

pub = rospy.Publisher("/car_vel", Floor, queue_size=2)
pid_pub = rospy.Publisher("/pid", String, queue_size=2)

lidar_sub = rospy.Subscriber("/scan", LaserScan, LidarCallback)
visual_sub = rospy.Subscriber("/visual", String, CamCtlCallback)
test_sub = rospy.Subscriber("/test", String, TestCtlCallback)

print("[INFO] Scan Start")

last_send_time = time.time()

if __name__ == "__main__":
    print("[MAIN]")
    try:
        while not rospy.is_shutdown():

            if time.time() < STOP_UNTIL or FLAG_STOP:
                angle = int(MID_ANGLE - steeringAngle)
                msg = Floor()
                msg.speed = 1500
                msg.angle = angle
                msg.headlight = 1 if headlight else 0
                msg.stop = 1
                pub.publish(msg)
                if FLAG_STOP:
                    print("[STOP] STOP")
                    rospy.rostime.wallsleep(0.01)
                    continue
                else:
                    print(f"[STOP] {STOP_UNTIL - time.time():.2f}s for stop")
                    rospy.rostime.wallsleep(0.01)
                    continue
            
            calced_speed = calcSpeedByAngle(steeringAngle, currentSpeed, speedRange)
            if isDataReceived:
                angle = int(MID_ANGLE - steeringAngle)
                msg = Floor()
                msg.speed = calced_speed
                msg.angle = angle
                msg.headlight = 1 if headlight else 0
                msg.stop = 0
                pub.publish(msg)
                realAngle = calcRealAngle(steeringAngle)
                print(
                    f"[{'R' if realAngle > 0 else 'L'}] Stop:{msg.stop} Error: {round(pid.last_error,2)} angle: {round(realAngle,2)} Speed:{msg.speed} Kp:{round(kp,4)} Ki:{round(ki,4)} Kd:{round(kd,4)}"
                )
                isDataReceived = False

            pid_data = {
                "error": round(pid.last_error,2),
                "angle": round(calcRealAngle(steeringAngle),2),
                "speed": calced_speed,
                "goal": 0,
                "kp": round(kp,4),
                "ki": round(ki,4),
                "kd": round(kd,4),
            }
            if time.time() - last_send_time > 0.1:
                pid_pub.publish(f"{pid_data['error']},{pid_data['kp']},{pid_data['ki']},{pid_data['kd']}\n")

            rospy.rostime.wallsleep(0.01)
    finally:
        msg = Floor()
        msg.speed = 1500
        msg.angle = MID_ANGLE
        msg.horn = 0
        msg.headlight = 0
        msg.stop = 1
        pub.publish(msg)
        os.system(STTY_DEF + " " + TTY_PATH)
